
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mode.c
  * @author     baiyang
  * @date       2021-8-12
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "fms.h"
#include "mode.h"

#include <parameter/param.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/
static UserTakeOff takeoff;
static AutoYaw auto_yaw;

// altitude above-ekf-origin below which auto takeoff does not control horizontal position
static bool auto_takeoff_no_nav_active;
static float auto_takeoff_no_nav_alt_cm;

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// returns a unique number specific to this mode
static ModeNumber mode_number() { return STABILIZE; }

// child classes should override these methods
static bool mode_init(bool ignore_checks) { return true; }
static void mode_exit() {}
static void mode_run() {}
static bool mode_requires_GPS() {return false;}
static bool mode_has_manual_throttle() { return false; }
static bool mode_allows_arming(ArmingMethod method) { return false; }
static bool mode_is_autopilot() { return false;}
static bool mode_has_user_takeoff(bool must_navigate) { return false; }
static bool mode_in_guided_mode() { return false; }
static bool mode_logs_attitude() { return false; }
static bool mode_allows_save_trim() { return false; }
static bool mode_allows_autotune() { return false; }
static bool mode_allows_flip() { return false; }

// return a string for this flightmode
static const char *mode_name() { return "?";}
static const char *mode_name4() { return "?";}

static bool is_taking_off() {return false;}
static bool is_landing() { return false;}

// mode requires terrain to be present to be functional
static bool requires_terrain_failsafe() { return false;}

// functions for reporting to GCS
static bool get_wp(Location *loc) { return false; };
static int32_t wp_bearing() { return 0; }
static uint32_t wp_distance() { return 0; }
static float crosstrack_error() { return 0.0f;}

// return expected input throttle setting to hover:
static float throttle_hover() {return ((MotorsMC_HandleTypeDef *)fms.motors)->_throttle_hover;}
static bool do_user_takeoff_start(float takeoff_alt_cm)
{
    //copter.flightmode->takeoff.start(takeoff_alt_cm);
    return true;
}

// 构造函数
void mode_ctor()
{
    mode_base.G_Dt = fms.G_Dt;
    mode_stabilize_ctor(&mode_base);
    mode_althold_ctor(&mode_base);
}

bool mode_is_disarmed_or_landed()
{
    if (!fms.motors->_armed || !fms.ap.auto_armed || fms.ap.land_complete) {
        return true;
    }
    return false;
}

void mode_zero_throttle_and_relax_ac(bool spool_up)
{
    if (spool_up) {
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);
    } else {
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_GROUND_IDLE);
    }
    attctrl_input_euler_angle_roll_pitch_euler_rate_yaw(fms.attitude_control, 0.0f, 0.0f, 0.0f);
    attctrl_set_throttle_out(fms.attitude_control,0.0f, false, PARAM_GET_FLOAT(VEHICLE,PILOT_THR_FILT));
}

void mode_zero_throttle_and_hold_attitude()
{
    // run attitude controller
    attctrl_input_euler_angle_roll_pitch_euler_rate_yaw(fms.attitude_control, 0.0f, 0.0f, 0.0f);
    attctrl_set_throttle_out(fms.attitude_control,0.0f, false, PARAM_GET_FLOAT(VEHICLE,PILOT_THR_FILT));
}

AltHoldModeState mode_get_alt_hold_state(float target_climb_rate_cms)
{
    // Alt Hold State Machine Determination
    if (!fms.motors->_armed) {
        // the aircraft should moved to a shut down state
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_SHUT_DOWN);

        // transition through states as aircraft spools down
        switch (fms.motors->_spool_state) {

        case MOTOR_SHUT_DOWN:
            return AltHold_MotorStopped;

        case MOTOR_GROUND_IDLE:
            return AltHold_Landed_Ground_Idle;

        default:
            return AltHold_Landed_Pre_Takeoff;
        }

    } else if (takeoff_running(&takeoff) || takeoff_triggered(target_climb_rate_cms)) {
        // the aircraft is currently landed or taking off, asking for a positive climb rate and in THROTTLE_UNLIMITED
        // the aircraft should progress through the take off procedure
        return AltHold_Takeoff;

    } else if (!fms.ap.auto_armed || fms.ap.land_complete) {
        // the aircraft is armed and landed
        if (target_climb_rate_cms < 0.0f && !fms.ap.using_interlock) {
            // the aircraft should move to a ground idle state
            Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_GROUND_IDLE);

        } else {
            // the aircraft should prepare for imminent take off
            Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);
        }

        if (fms.motors->_spool_state == MOTOR_GROUND_IDLE) {
            // the aircraft is waiting in ground idle
            return AltHold_Landed_Ground_Idle;

        } else {
            // the aircraft can leave the ground at any time
            return AltHold_Landed_Pre_Takeoff;
        }

    } else {
        // the aircraft is in a flying state
        Motors_set_desired_spool_state(fms.motors, MOTOR_DESIRED_THROTTLE_UNLIMITED);
        return AltHold_Flying;
    }

}

void mode_set_throttle_takeoff()
{
    // initialise the vertical position controller
    posctrl_init_z_controller(fms.pos_control);
}

Mode mode_base = {
        .mode_number = mode_number,
        .init        = mode_init,
        .exit        = mode_exit,
        .run         = mode_run,
        .requires_GPS = mode_requires_GPS,
        .has_manual_throttle = mode_has_manual_throttle,
        .allows_arming = mode_allows_arming,
        .is_autopilot = mode_is_autopilot,
        .has_user_takeoff = mode_has_user_takeoff,
        .in_guided_mode = mode_in_guided_mode,
        .logs_attitude = mode_logs_attitude,
        .allows_save_trim = mode_allows_save_trim,
        .allows_autotune = mode_allows_autotune,
        .allows_flip = mode_allows_flip,
        .name = mode_name,
        .name4 = mode_name4,
        .is_taking_off = is_taking_off,
        .is_landing = is_landing,
        .requires_terrain_failsafe = requires_terrain_failsafe,
        .get_wp = get_wp,
        .wp_bearing = wp_bearing,
        .wp_distance = wp_distance,
        .crosstrack_error = crosstrack_error,
        .throttle_hover = throttle_hover,
        .do_user_takeoff_start = do_user_takeoff_start,
        .takeoff = &takeoff,
        .auto_yaw = &auto_yaw,
        .auto_takeoff_no_nav_active = &auto_takeoff_no_nav_active,
        .auto_takeoff_no_nav_alt_cm = &auto_takeoff_no_nav_alt_cm};

/*------------------------------------test------------------------------------*/


